#include "System.h"



SIGNED_WORD ByteStackPush(ByteStack * Stack,BYTE Val)
{
    if(Stack->Ptr == Stack->Size-1)
    {
        return STACK_FULL;
    }
    else
    {
        Stack->Ptr++;
        Stack->StackSpace[Stack->Ptr] = Val;
        return STACK_PUSH_OK;
    }
}

SIGNED_WORD ByteStackPOP(ByteStack * Stack)
{
    if(Stack->Ptr == 0)
    {
        return STACK_EMPTY;
    }
    else
    {
        Stack->Ptr--;
        return Stack->StackSpace[Stack->Ptr];
    }
}

SIGNED_BYTE BitStackPush(BitStack * Stack,BOOL Val)
{
    WORD Offset;
    BYTE Mask;
    
    if(Stack->Ptr == Stack->Size-1)
    {
        return STACK_FULL;
    }
    else
    {
        
        Stack->Ptr++;
        Offset = (Stack->Ptr)>>3;
        Mask = 0x01<<(Stack->Ptr&0x07);
        
        if(Val)
        {
            Stack->StackSpace[Offset] |= Mask;    
        }
        else
        {
            Stack->StackSpace[Offset] &= ~Mask;
        }
        
        return STACK_PUSH_OK;
    }
}

SIGNED_BYTE BitStackPop(BitStack * Stack)
{
    WORD Offset;
    BYTE Mask;
    
    if(Stack->Ptr == 0)
    {
        return STACK_EMPTY;
    }
    else
    {
        
        Stack->Ptr++;
        Offset = (Stack->Ptr)>>3;
        Mask = 0x01<<(Stack->Ptr&0x07);
        
        if(Stack->StackSpace[Offset] | Mask)
        {
            return TRUE;
        }        
        else
        {
            return FALSE;
        }
    }
}

void BitPlane_Put(BitPlane  * BP, WORD X,WORD Y, BOOL Value)
{
    WORD Offset;
    BYTE Mask;
    
    Offset = (Y * ((BP->SizeX)>>3)) + (X>>3);
    Mask = 0x01 << (X & 0x07);

    if(Value)
    {
        BP->BitPlaneSpace[Offset] |= Mask;
    }
    else
    {
        BP->BitPlaneSpace[Offset] &= ~Mask;
    }
}


BOOL BitPlane_Get(BitPlane  * BP, WORD X,WORD Y)
{
    WORD Offset;
    BYTE Mask;
    
	
    Offset = (Y * ((BP->SizeX)>>3)) + (X>>3);
    Mask = 0x01 << (X & 0x07);

    if((BP->BitPlaneSpace[Offset])&Mask)
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
    
}

void BitPlane_Clear(BitPlane  * BP)
{
    WORD PlaneSpaceSize;
    WORD i;

	PlaneSpaceSize = ((BP->SizeX)>>3) * BP->SizeY;
    
	for(i=0;i<PlaneSpaceSize;i++)
	{
		BP->BitPlaneSpace[i] = 0;
	}
}



void InitByteQueue(ByteQueue *BQ,WORD Size,BYTE * Storage)
{
	WORD i;

	BQ->QueueSize = Size;
	BQ->ReadPtr=0;
	BQ->WritePtr=0;
	BQ->QueueStorage = Storage;

	for(i=0;i<BQ->QueueSize;i++)
	{
		BQ->QueueStorage[i] = 0;
	}
}

WORD BytesInQueue(ByteQueue *BQ)
{
	if(BQ->ReadPtr > BQ->WritePtr)
	{
		return (BQ->QueueSize - BQ->ReadPtr + BQ->WritePtr);
	}
	else if (BQ->WritePtr > BQ->ReadPtr)
	{
 		return 	(BQ->WritePtr - BQ->ReadPtr);
	}
	else
	{
		return 0;	
	}
}

SIGNED_WORD ByteEnqueue(ByteQueue *BQ,BYTE Val)
{

	if(BytesInQueue(BQ) == BQ->QueueSize)
	{
		return QUEUE_FULL;
	}
	else
	{
		BQ->QueueStorage[BQ->WritePtr] = Val;
		BQ->WritePtr++;
		
		if(BQ->WritePtr >= BQ->QueueSize)
			{
				BQ->WritePtr = 0;	
			}
		return QUEUE_OK;
	}
}


SIGNED_WORD SerialByteEnqueue(ByteQueue *BQ,BYTE Val)
{
  #ifdef ACTION_BEFORE_SERIAL_BYTE_ENQUEUE
         ACTION_BEFORE_SERIAL_BYTE_ENQUEUE;
  #endif
  
	if(BytesInQueue(BQ) == BQ->QueueSize)
	{
		return QUEUE_FULL;
	}
	else
	{
		BQ->QueueStorage[BQ->WritePtr] = Val;
		BQ->WritePtr++;
		
		if(BQ->WritePtr >= BQ->QueueSize)
			{
				BQ->WritePtr = 0;	
			}
		return QUEUE_OK;
	}
}

SIGNED_WORD ByteArrayEnqueue(ByteQueue *BQ,BYTE *Buf,WORD Len)
{
	WORD i;
	for(i=0;i<Len;i++)
	{
		(void)SerialByteEnqueue(BQ,Buf[i]);
	}
	return QUEUE_OK;
}

SIGNED_WORD SerialByteArrayEnqueue(ByteQueue *BQ,BYTE *Buf,WORD Len)
{
	WORD i;
	for(i=0;i<Len;i++)
	{
		(void)SerialByteEnqueue(BQ,Buf[i]);
	}
	return QUEUE_OK;
}

SIGNED_WORD ByteDequeue(ByteQueue *BQ,BYTE *Val)
{
	
	if(BytesInQueue(BQ) == 0)
	{
		return QUEUE_EMPTY;
	}
	else
	{
		*Val  = BQ->QueueStorage[BQ->ReadPtr];

		BQ->ReadPtr++;

		if(BQ->ReadPtr >= BQ->QueueSize)
		{
			BQ->ReadPtr = 0;	
		}
		return QUEUE_OK;
	}
}

